#include <stdint.h>
#include <string.h>
#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/uuid.h>
#include <zephyr/bluetooth/conn.h>
#include <zephyr/bluetooth/gatt.h>
#include <zephyr/bluetooth/uuid.h>
#include <zephyr/kernel.h>
#include <zephyr/sys/printk.h>

#include "ble_gatt_discovery.h"

static struct GATTDevice *gatt_device = NULL;
static int current_service_index = 0;
static gatt_discovery_complete_cb user_callback = NULL;
static enum discovery_status_t discovery_status = DISCOVERY_SUCCESS;

static void discover_characteristics(struct bt_conn *conn);
static void discover_descriptors(struct bt_conn *conn);

static void *k_realloc(void *ptr, size_t new_size)
{
    if (new_size == 0)
    {
        k_free(ptr);
        return NULL;
    }

    if (!ptr)
    {
        return k_malloc(new_size);
    }

    void *new_ptr = k_malloc(new_size);
    if (!new_ptr)
    {
        return NULL; // 分配失败
    }

    // 将旧数据复制到新地址
    memcpy(new_ptr, ptr, new_size);

    k_free(ptr); // 释放旧的内存块
    return new_ptr;
}

// 动态分配并复制 UUID 数据
static struct bt_uuid *copy_uuid(const struct bt_uuid *src_uuid)
{
    struct bt_uuid *new_uuid = NULL;

    // 判断 UUID 的类型
    if (src_uuid->type == BT_UUID_TYPE_16) {
        new_uuid = k_malloc(sizeof(struct bt_uuid_16));
        if (new_uuid) {
            memcpy(new_uuid, src_uuid, sizeof(struct bt_uuid_16));
        }
    } else if (src_uuid->type == BT_UUID_TYPE_128) {
        new_uuid = k_malloc(sizeof(struct bt_uuid_128));
        if (new_uuid) {
            memcpy(new_uuid, src_uuid, sizeof(struct bt_uuid_128));
        }
    } else {
        printk("Unknown UUID type\n");
    }

    return new_uuid;
}


static void print_uuid(const struct bt_uuid *uuid)
{
    char uuid_str[BT_UUID_STR_LEN];
    bt_uuid_to_str(uuid, uuid_str, sizeof(uuid_str));
    printk("%s", uuid_str);
}

// 初始化 GATT 设备结构体
static struct GATTDevice *gatt_device_init(void)
{
    struct GATTDevice *device = k_malloc(sizeof(struct GATTDevice));
    if (!device)
    {
        printk("Failed to allocate memory for GATT device\n");
        return NULL;
    }
    device->services = NULL;
    device->num_services = 0;
    return device;
}

// 释放 GATT 设备结构体及其内部的动态内存
static void gatt_device_free(struct GATTDevice *device)
{
    if (!device) return;

    // 释放每个服务的内存
    for (int i = 0; i < device->num_services; i++) {
        struct GATTService *service = &device->services[i];

        // 释放服务的 UUID
        if (service->uuid) {
            k_free(service->uuid);
        }

        // 释放每个特征的内存
        for (int j = 0; j < service->num_characteristics; j++) {
            struct GATTCharacteristic *chrc = &service->characteristics[j];

            // 释放特征的 UUID
            if (chrc->uuid) {
                k_free(chrc->uuid);
            }

            // 释放每个描述符的内存
            for (int k = 0; k < chrc->num_descriptors; k++) {
                struct GATTDescriptor *desc = &chrc->descriptors[k];

                // 释放描述符的 UUID
                if (desc->uuid) {
                    k_free(desc->uuid);
                }
            }

            // 释放描述符数组
            if (chrc->descriptors) {
                k_free(chrc->descriptors);
            }
        }

        // 释放特征数组
        if (service->characteristics) {
            k_free(service->characteristics);
        }
    }

    // 释放服务数组
    k_free(device->services);

    // 释放设备结构体
    k_free(device);
}


// 动态扩展服务数组
static struct GATTService *add_service(struct GATTDevice *device)
{
    struct GATTService *new_services = k_realloc(device->services, (device->num_services + 1) * sizeof(struct GATTService));
    if (!new_services)
    {
        printk("Failed to reallocate memory for services\n");
        discovery_status = DISCOVERY_FAILED;
        return NULL;
    }
    device->services = new_services;
    struct GATTService *new_service = &device->services[device->num_services];
    new_service->characteristics = NULL;
    new_service->num_characteristics = 0;
    new_service->uuid = NULL;
    device->num_services++;
    return new_service;
}

// 动态扩展特征数组
static struct GATTCharacteristic *add_characteristic(struct GATTService *service)
{
    struct GATTCharacteristic *new_characteristics = k_realloc(service->characteristics,
                                                               (service->num_characteristics + 1) * sizeof(struct GATTCharacteristic));
    if (!new_characteristics)
    {
        printk("Failed to reallocate memory for characteristics\n");
        discovery_status = DISCOVERY_FAILED;
        return NULL;
    }
    service->characteristics = new_characteristics;
    struct GATTCharacteristic *new_chrc = &service->characteristics[service->num_characteristics];
    new_chrc->descriptors = NULL;
    new_chrc->num_descriptors = 0;
    service->num_characteristics++;
    return new_chrc;
}

// 动态扩展描述符数组
static struct GATTDescriptor *add_descriptor(struct GATTCharacteristic *chrc)
{
    struct GATTDescriptor *new_descriptors = k_realloc(chrc->descriptors, (chrc->num_descriptors + 1) * sizeof(struct GATTDescriptor));
    if (!new_descriptors)
    {
        printk("Failed to reallocate memory for descriptors\n");
        discovery_status = DISCOVERY_FAILED;
        return NULL;
    }
    chrc->descriptors = new_descriptors;
    struct GATTDescriptor *new_desc = &chrc->descriptors[chrc->num_descriptors];
    chrc->num_descriptors++;
    return new_desc;
}

// 服务发现的回调函数
static uint8_t discover_service_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr,
                                   struct bt_gatt_discover_params *params)
{
    if (!attr) {
        // 服务发现完成
        printk("Service discovery complete.\n");
        current_service_index = 0;
        discover_characteristics(conn);  // 开始查找特征
        return BT_GATT_ITER_STOP;
    }

    // 新增服务
    struct GATTService *service = add_service(gatt_device);
    if (!service) return BT_GATT_ITER_STOP;

    service->start_handle = attr->handle;
    service->end_handle = ((struct bt_gatt_service_val *)attr->user_data)->end_handle;

    // 复制 UUID 数据
    service->uuid = copy_uuid(((struct bt_gatt_service_val *)attr->user_data)->uuid);
    if (!service->uuid) {
        printk("Failed to allocate memory for service UUID\n");
        discovery_status = DISCOVERY_FAILED;
        gatt_device_free(gatt_device);  // 释放已经分配的内存
        return BT_GATT_ITER_STOP;
    }

    // printk("Service found: UUID ");
    // print_uuid(service->uuid);
    // printk(", Handle Range: 0x%04x - 0x%04x\n", service->start_handle, service->end_handle);

    return BT_GATT_ITER_CONTINUE;
}

// 特征发现的回调函数
static uint8_t discover_char_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr,
                                struct bt_gatt_discover_params *params)
{
    if (!attr)
    {
        discover_descriptors(conn);
        return BT_GATT_ITER_STOP;
    }

    struct GATTService *service = &gatt_device->services[current_service_index];
    struct GATTCharacteristic *chrc = add_characteristic(service);
    if (!chrc)
        return BT_GATT_ITER_STOP;

    struct bt_gatt_chrc *bt_chrc = (struct bt_gatt_chrc *)attr->user_data;
    chrc->handle = attr->handle;
    chrc->value_handle = bt_chrc->value_handle;
    chrc->properties = bt_chrc->properties;

    // 复制 UUID 数据
    chrc->uuid = copy_uuid(bt_chrc->uuid);
    if (!chrc->uuid) {
        printk("Failed to allocate memory for characteristic UUID\n");
        discovery_status = DISCOVERY_FAILED;
        gatt_device_free(gatt_device);  // 释放已经分配的内存
        return BT_GATT_ITER_STOP;
    }

    // printk("Characteristic found: UUID ");
    // print_uuid(chrc->uuid);
    // printk(", Handle: 0x%04x, Value Handle: 0x%04x\n", chrc->handle, chrc->value_handle);

    return BT_GATT_ITER_CONTINUE;
}

// 描述符发现的回调函数
static uint8_t discover_desc_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr,
                                struct bt_gatt_discover_params *params)
{
    if (!attr)
    {
        current_service_index++;
        if (current_service_index < gatt_device->num_services)
        {
            discover_characteristics(conn);
        }
        else
        {
            // 所有服务、特征和描述符查找完成，回调通知用户
            if (user_callback)
            {
                user_callback(gatt_device, discovery_status);
            }
            gatt_device_free(gatt_device); // 自动释放资源
        }
        return BT_GATT_ITER_STOP;
    }

    struct GATTService *service = &gatt_device->services[current_service_index];
    struct GATTCharacteristic *chrc = &service->characteristics[service->num_characteristics - 1];
    struct GATTDescriptor *desc = add_descriptor(chrc);
    if (!desc)
        return BT_GATT_ITER_STOP;

    desc->handle = attr->handle;

    // 复制 UUID 数据
    desc->uuid = copy_uuid(attr->uuid);
    if (!desc->uuid) {
        printk("Failed to allocate memory for descriptor UUID\n");
        discovery_status = DISCOVERY_FAILED;
        gatt_device_free(gatt_device);  // 释放已经分配的内存
        return BT_GATT_ITER_STOP;
    }

    printk("Descriptor found: UUID ");
    print_uuid((struct bt_uuid *)attr->uuid);
    printk(", Handle: 0x%04x\n", desc->handle);

    return BT_GATT_ITER_CONTINUE;
}

// 启动查找服务
static void discover_services(struct bt_conn *conn)
{
    static struct bt_gatt_discover_params params;
    memset(&params, 0, sizeof(params));
    params.uuid = NULL;
    params.start_handle = 0x0001;
    params.end_handle = 0xFFFF;
    params.type = BT_GATT_DISCOVER_PRIMARY;
    params.func = discover_service_cb;

    int err = bt_gatt_discover(conn, &params);
    if (err)
    {
        printk("Service discovery failed (err %d)\n", err);
        discovery_status = DISCOVERY_FAILED;
        // 立即调用回调通知错误
        if (user_callback)
        {
            user_callback(NULL, DISCOVERY_FAILED);
        }
        // 清理资源
        gatt_device_free(gatt_device);
    }
}

// 启动查找特征
static void discover_characteristics(struct bt_conn *conn)
{
    struct GATTService *service = &gatt_device->services[current_service_index];
    static struct bt_gatt_discover_params params;
    memset(&params, 0, sizeof(params));
    params.uuid = NULL;
    params.start_handle = service->start_handle + 1;
    params.end_handle = service->end_handle;
    params.type = BT_GATT_DISCOVER_CHARACTERISTIC;
    params.func = discover_char_cb;

    int err = bt_gatt_discover(conn, &params);
    if (err)
    {
        printk("Characteristic discovery failed (err %d)\n", err);
        discovery_status = DISCOVERY_FAILED;
    }
}

// 启动查找描述符
static void discover_descriptors(struct bt_conn *conn)
{
    struct GATTService *service = &gatt_device->services[current_service_index];
    // struct GATTCharacteristic *chrc = &service->characteristics[service->num_characteristics - 1];

    static struct bt_gatt_discover_params params;
    memset(&params, 0, sizeof(params));
    params.uuid = NULL;
    params.start_handle = service->start_handle + 1;
    params.end_handle = service->end_handle;
    params.type = BT_GATT_DISCOVER_DESCRIPTOR;
    params.func = discover_desc_cb;

    int err = bt_gatt_discover(conn, &params);
    if (err)
    {
        printk("Descriptor discovery failed (err %d)\n", err);
        discovery_status = DISCOVERY_FAILED;
    }
}

// 启动服务发现
void gatt_discover(struct bt_conn *conn, gatt_discovery_complete_cb callback)
{
    if (!conn)
    {
        printk("Invalid Bluetooth connection\n");
        if (callback)
        {
            callback(NULL, DISCOVERY_FAILED);
        }
        return;
    }

    // 初始化 GATT 设备
    gatt_device = gatt_device_init();
    if (!gatt_device)
    {
        if (callback)
        {
            callback(NULL, DISCOVERY_FAILED);
        }
        return;
    }

    // 保存用户回调
    user_callback = callback;

    // 启动服务发现
    discover_services(conn);
}

// 打印服务、特征和描述符的结构信息
void print_device_gatt_structure(struct GATTDevice *device)
{
    if (!device)
    {
        printk("No GATT device found.\n");
        return;
    }

    // 遍历所有服务
    for (int i = 0; i < device->num_services; i++)
    {
        struct GATTService *service = &device->services[i];

        char uuid_str[BT_UUID_STR_LEN];
        bt_uuid_to_str(service->uuid, uuid_str, sizeof(uuid_str));
        printk("Service [%d]: UUID: %s, Handle Range: 0x%04x - 0x%04x\n", i + 1,
               uuid_str, service->start_handle, service->end_handle);

        // 遍历该服务下的所有特征
        for (int j = 0; j < service->num_characteristics; j++)
        {
            struct GATTCharacteristic *chrc = &service->characteristics[j];

            bt_uuid_to_str(chrc->uuid, uuid_str, sizeof(uuid_str));
            printk("  Characteristic [%d]: UUID: %s, Handle: 0x%04x, Value Handle: 0x%04x, Properties: 0x%02x\n",
                   j + 1, uuid_str, chrc->handle, chrc->value_handle, chrc->properties);

            // 遍历该特征下的所有描述符
            for (int k = 0; k < chrc->num_descriptors; k++)
            {
                struct GATTDescriptor *desc = &chrc->descriptors[k];

                bt_uuid_to_str(desc->uuid, uuid_str, sizeof(uuid_str));
                printk("    Descriptor [%d]: UUID: %s, Handle: 0x%04x\n", k + 1, uuid_str, desc->handle);
            }
        }
    }
}
